/**************************************************************************//**
 * @item     CosyOS-II Config
 * @file     mcucfg_cmx.c
 * @brief    CMSIS Cortex-M Core Config File
 * @author   迟凯峰
 * @version  V1.0.0
 * @date     2023.10.20
 ******************************************************************************/

#include "..\System\os_var.h"
#ifdef __MCUCFG_CMX_H

s_u32_t  m_basepri = 1;
#if SYSCFG_TASKMSG == __ENABLED__
s_u32_t *m_taskmsg_psp;
#endif



/*
 * 任务临界区
 */

void mu_enter_critical(void)
{
	#if !MCUCFG_PENDSV
	__asm("msr basepri, m_basepri");
	__asm("dsb");
	__asm("isb");
	#else
	mPendSV_Disable;
	__asm("dsb");
	__asm("isb");
	mSysTick_Disable;
	__asm("dsb");
	__asm("isb");
	#endif
}

#if MCUCFG_PENDSV

void mu_exit_critical(void)
{
	mSysTick_Enable;
	mPendSV_Enable;
}

#endif

#if SYSCFG_TASKCRITICAL_RECURSION == __ENABLED__
static volatile s_u8_t m_taskcri_counter = 0;

void mu_reenter_critical(void)
{
	#if !MCUCFG_PENDSV
	__asm("msr basepri, m_basepri");
	__asm("dsb");
	__asm("isb");
	#else
	mPendSV_Disable;
	__asm("dsb");
	__asm("isb");
	mSysTick_Disable;
	__asm("dsb");
	__asm("isb");
	#endif
	m_taskcri_counter++;
}

void mu_reexit_critical(void)
{
	m_taskcri_counter--;
	if(!m_taskcri_counter){
		#if !MCUCFG_PENDSV
		register s_u32_t zero = 0;
		__asm("msr basepri, zero");
		#else
		mSysTick_Enable;
		mPendSV_Enable;
		#endif
	}
}

#endif



/*
 * 全局临界区
 */

#if MCUCFG_PENDSV

s_u32_t mx_masking_pri(s_u32_t newpri)
{
	register s_u32_t oldpri;
	newpri <<= m_basepri;
	__asm("mrs oldpri, basepri");
	__asm("msr basepri_max, newpri");
	__asm("dsb");
	__asm("isb");
	return oldpri;
}

#endif



/*
 * 中断挂起服务
 */

#if MCUCFG_PENDSVFIFO_DEPTH > 0
#if SYSCFG_PENDSVFIFO_MONITOR == __ENABLED__
s_u8_t mPendSV_FIFO_DepthMAX = 0;
#endif
void *mPendSV_FIFO[2][MCUCFG_PENDSVFIFO_DEPTH + 1];
register void **mPendSV_P0 __ASM("r10");
register void **mPendSV_P1 __ASM("r11");
volatile s_bool_t m_sign_fifo = true;

__ASM void mPendSV_Loader(void *addr)
{
	extern m_sign_fifo;
	
	ldr r1, =m_sign_fifo
	ldrb r2, [r1]
	cbz r2, __FIFO1
	
__FIFO0
	str r0, [r10,#4]!
	b __RET
	
__FIFO1
	str r0, [r11,#4]!
	
__RET
	bx lr
	nop
}

void mPendSV_Handler(void)
{
	register void **p;
	register void *sv;
	register s_u8_t depth;
__LABLE:
	m_sign_fifo = false;
	
	#if SYSCFG_PENDSVFIFO_MONITOR == __ENABLED__
	depth = (s_u8_t)(mPendSV_P0 - mPendSV_FIFO[0]);
	if(depth > mPendSV_FIFO_DepthMAX){
		mPendSV_FIFO_DepthMAX = depth;
	}
	#endif
	p = mPendSV_FIFO[0];
	do{
		sv = *++p;
		(*sPendSV_Handler[*(const s_u8_t *)sv])(sv);
	}while(mPendSV_P0 > p);
	mPendSV_P0 = mPendSV_FIFO[0];
	
	m_sign_fifo = true;
	if(mPendSV_P1 == mPendSV_FIFO[1]) return;
	
	#if SYSCFG_PENDSVFIFO_MONITOR == __ENABLED__
	depth = (s_u8_t)(mPendSV_P1 - mPendSV_FIFO[1]);
	if(depth > mPendSV_FIFO_DepthMAX){
		mPendSV_FIFO_DepthMAX = depth;
	}
	#endif
	p = mPendSV_FIFO[1];
	do{
		sv = *++p;
		(*sPendSV_Handler[*(const s_u8_t *)sv])(sv);
	}while(mPendSV_P1 > p);
	mPendSV_P1 = mPendSV_FIFO[1];
	
	if(mPendSV_P0 > mPendSV_FIFO[0]) goto __LABLE;
}

#endif

__ASM void mTaskStack_PUSHPOP(s_bool_t f, void *p)
{
	extern s_task_current;
	extern s_pc;
	extern s_taskmgr_binary;
	
	PRESERVE8
	isb
	ldr r2, =s_task_current
	cbz r0, __POP
	mrs r0, psp
	
__TASKPC
	IF SYSCFG_TASKPC_MONITOR == __ENABLED__
	ldr r3, =s_taskmgr_binary
	ldrb r3, [r3]
	cbz r3, __PUSH
	mov r3, r0
	add r3, #24
	ldmia r3, {r2}
	ldr r3, =s_pc
	str r2, [r3]
	ldr r2, =s_task_current
	ENDIF
	
__PUSH
	IF MCUCFG_HARDWAREFPU == __ENABLED__
	vstmdb r0!, {s16-s31}
	ENDIF
	stmdb r0!, {r4-r9}
	ldr r3, [r2]
	str r0, [r3]
	
__POP
	str r1, [r2]
	ldr r0, [r1]
	ldmia r0!, {r4-r9}
	IF MCUCFG_HARDWAREFPU == __ENABLED__
	vldmia r0!, {s16-s31}
	ENDIF
	msr psp, r0
	bx lr
	nop
}



#endif
